#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_tf_broadcaster");
    ros::NodeHandle node;
    //定义一个tf发布器
    tf::TransformBroadcaster br;
    //定义一个tf变量
    tf::Transform transform;
    //定义三角形的边长为1
    float triangle_scale = 1.0;
    //从参数服务器中获取指定的三角形边长
    ros::param::get("triangle_scale",triangle_scale);
    //定义运行周期为10hz
    ros::Rate rate(10.0);
    while (node.ok())
    {
        //指定x、y坐标其中x=-边长*sin60°，y= -边长*cos60°
        transform.setOrigin( tf::Vector3(-triangle_scale/2*sqrt(3), -triangle_scale/2, 0) );
        transform.setRotation( tf::	Quaternion(0, 0, 0, 1) );
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
        //指定x、y坐标其中x=-边长*sin60°，y= 边长*cos60°
        transform.setOrigin( tf::Vector3(-triangle_scale/2*sqrt(3), triangle_scale/2, 0) );
        transform.setRotation( tf::	Quaternion(0, 0, 0, 1) );
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot2"));
        rate.sleep();  
    }
    return 0;
}